/* GPS interface */

/* Copyright (C) 2009-2012 David Zanetti
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; version 2 of the License.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this program; if not, write to the Free Software Foundation, Inc.,
 *  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */


#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <avr/wdt.h>
#include <math.h>

#include "global.h"
#include <util/delay.h>

#include "rtc.h"
#include "serial.h"
#include "gps.h"
#include "ds3234.h"

/* GPS parser states */
#define GPS_STATE_INIT 0
#define GPS_STATE_START 1
#define GPS_STATE_PAYLOAD 2
#define GPS_STATE_COMPLETE 3

#define GPS_PKTLEN 128

//ISR(PCINT1_vect) {
// if (PINB & (1 << PINB0)) {
//  if (!(flags & FLAG_GPS_PPS)) {
//   flags |= FLAG_GPS_PPS;
//   clock_us_pps = TCNT1;
//   clock_ms_pps = clock_frac;
//   clock_s_gps = clock_s;
//  }
// }
//}

// ISR(INT2_vect) {
// clock_us_pps = TCNT1;
// clock_ms_pps = clock_frac;
// clock_s_pps = clock_s;
// clock_s_gps = 0;
// clock_ms_gps = 0;
// flags |= FLAG_GPS_PPS; /* we are allowed to do this as we're in an interrupt */
// }

/* Handle our PPS interrupts. Since this is only wired to one pin, just assume it's the PPS pin */
ISR(PORTD_INT0_vect) {
	/* save the RTC state, for compares later */
#ifdef RTC_H
	//rtc_ms_pps = RTC.CNT;
	//rtc_s_pps = rtc_s;
#endif
#ifdef DS3234_H
	/* save the upper byte and era of the clock */
	/* fixme: how do grab this glitch-free when timer clock rolls over */
	ds3234_msb_s_pps = ds3234_msb_s;
	ds3234_era_pps = ds3234_era;
#endif
	PORTC.OUTTGL = PIN1_bm;
	flag_gps |= FLAG_GPS_PPS; /* safe since we're in a high prio interrupt */
}

void gps_requestversion(void) {
	serial1_tx_cout(0x10);
	serial1_tx_cout(0x1c);
	serial1_tx_cout(0x03);
	serial1_tx_cout(0x10);
	serial1_tx_cout(0x03);
	return;
}

void gps_init(void) {

	/* make sure PPS flag is not set */
	//clear_flag(FLAG_GPS_PPS);

	serial0_tx_PGM(PSTR("gps: init\r\n"));

	/* prepare our serial port first */
	serial1_init();

	/* configure the PPS pin to act as an interrupt. */
	PORTD.DIRCLR = PIN1_bm; /* should be input */
	PORTD.PIN1CTRL = PORT_ISC_RISING_gc; /* sense rising edge */

	//PORTD.PIN1CTRL = PORT_ISC_RISING_gc | PORT_OPC_PULLDOWN_gc; /* sense on rising edge, with built-in pull down */
	EVSYS.CH0MUX = EVSYS_CHMUX_PORTD_PIN1_gc; /* mux pin changes into channel 0 */
	PORTD.INT0MASK = PIN1_bm; /* allocate pin1 to port d interrupt 0 */
	PORTD.INTCTRL = PORT_INT0LVL_HI_gc; /* now fire interrupts for it, hi priority */
	PMIC.CTRL |= PMIC_HILVLEX_bm; /* ensure high-level interrupts are enabled, still need global interrupt enabled somewhere */

	/* make sure serial port is settled before bringing it out of reset */
	_delay_ms(100);

	/* bring chip out of reset */
	PORTD.DIRSET = PIN4_bm;

	/* pull low for 1s, to ensure if we crashed into this state that it is actually reset */
	PORTD.OUTCLR = PIN4_bm;
	_delay_ms(100);
	/* flush any data in the port */
	serial1_flush();
	_delay_ms(100);
	PORTD.OUTSET = PIN4_bm; /* now out of reset */

	// DDRB |= (1 << PORTB1); /* GPS Status LED */



	/* silence packets we don't care about seeing automatically */
	// serial1_tx_PGM(PSTR("\x10\x35\x00\x00\x10\x10\x00\x10\x03"));

	//serial1_tx_cout(0x10); /* command start */
	//serial1_tx_cout(0x35); /* Set/Request IO Options, pg 127 */
	//serial1_tx_cout(0x00); /* Disable position reporting */
	//serial1_tx_cout(0x00); /* Disable velocity reporting */
	//serial1_tx_cout(0x10); /* PPS Mode fixed based, GPS time */
	//serial1_tx_cout(0x10); /* exscaping of end sequence */
	//serial1_tx_cout(0x00); /* no raw measurements, signal level AMU, signal off */
	//serial1_tx_cout(0x10);
	//serial1_tx_cout(0x03); /* end packet */

}

void gps_request_utc(void) {
	serial1_tx_cout(0x10);
	serial1_tx_cout(0x38);
	serial1_tx_cout(0x01);
	serial1_tx_cout(0x05);
	serial1_tx_cout(0x00);
	serial1_tx_cout(0x10);
	serial1_tx_cout(0x03);
	//serial1_tx_PGM(PSTR("\x10\x38\x01\x05\x00\x10\x03"));
}

void gps_process(void) {
	static uint8_t pkt[GPS_PKTLEN+1];
	static uint8_t n = 0, state = GPS_STATE_START, dle_seen = 0;
	static uint8_t s;

	/* assume we are starting clean, based on power-up reset */

	//serial0_tx_PGM(PSTR("gps: gps_process() called\r\n"));

	switch (state) {
		case GPS_STATE_INIT:
			/* skip packets until we see the correct end sequence */
			s = serial1_rx();
			if (s == 0x10) {
				dle_seen = 1 - dle_seen;
				/* we're done for now, since we're in init */
				break;
			}
			if (s == 0x03 && dle_seen) {
				serial0_tx_PGM(PSTR("gps: tsip parser resynced\r\n"));
				dle_seen = 0;
				state = GPS_STATE_START;
				n = 0;
				break;
			}
			dle_seen = 0;
			break;
		case GPS_STATE_START:
			/* packet is a start packet, right? */
			s = serial1_rx();
			if (s != 0x10) {
				serial0_tx_PGM(PSTR("gps: out of sync, resyncing\r\n"));
				/* we're out of sync, give up */
				state = GPS_STATE_INIT;
				break;
			}
			state = GPS_STATE_PAYLOAD;
			break;
		case GPS_STATE_PAYLOAD:
			/* copy the payload of the packet */
			s = serial1_rx();
			if (s == 0x10) {
				if (!dle_seen) {
					pkt[n++] = s;
				}
				dle_seen = 1 - dle_seen;
				break;
			}
			if (s == 0x03 && dle_seen) {
				n--; /* we had a DLE just before, not packet of packet */
				state = GPS_STATE_COMPLETE;
				dle_seen = 0;
				break;
			}
			pkt[n++] = s; /* copy the packet */
			dle_seen = 0;
			break;
		default:
			serial0_tx_PGM(PSTR("gps: BUG: parse error\r\n"));
			break;
	}

	/* process the completed packet if we have one */
	if (state == GPS_STATE_COMPLETE) {

		//serial0_tx_PGM(PSTR("gps: rx type "));
		//serial0_tx_hex(pkt[0]);
		//serial0_tx_cr();

		/* handle this packet */
		switch (pkt[0]) {
			case 0x1c:
				/* hardware version packet. we use this to validate we're talking to the GPS */
				if (pkt[1] != 0x83) {
					serial0_tx_PGM(PSTR("gps: bad hw version reply, closing connection to gps"));
					clear_flag(flag_gps,FLAG_GPS_GOOD);
					global_counters.gps_frame_bad++;
					break;
				}
				serial0_tx_PGM(PSTR("gps: found "));
				serial0_tx((char *)(pkt+14),pkt[13]);
				serial0_tx_PGM(PSTR(" sn "));
				serial0_tx_hex(pkt[2]);
				serial0_tx_hex(pkt[3]);
				serial0_tx_hex(pkt[4]);
				serial0_tx_hex(pkt[5]);
				serial0_tx_cout(' ');
				serial0_tx_dec((pkt[8] << 8) + pkt[9]);
				serial0_tx_cout('/');
				serial0_tx_dec(pkt[7]);
				serial0_tx_cout('/');
				serial0_tx_dec(pkt[6]);
				serial0_tx_cr();

				global_counters.gps_frame_good++;

				set_flag(flag_gps,FLAG_GPS_GOOD);

				/* send our requested config */
				/* FIXME: why is this gps config invalid? */
				//serial1_tx_PGM(PSTR("\x10\x35\x00\x00\x20\x00\x10\x03"));
				break;
#ifdef USE_GPS_41
			case 0x41:
				/* time packet */
				/* pg 130 */
				if (!(flag_gps & FLAG_GPS_LOCK)) {
					break; /* we ignore these packets without a full lock */
				}

				/* if we don't have a PPS flag, then we ignore this as a rogue packet */
				if (!(flag_gps & FLAG_GPS_PPS)) {
					serial0_tx_PGM(PSTR("gps: got 0x41; no PPS signal?\r\n"));
					global_counters.gps_frame_ignored++;
					break;
				}

				/* clear flag, we've got the first and only packet for this second, right? */
				clear_flag(flag_gps,FLAG_GPS_PPS);

				/* decode the time into clock*gps variables */

				/* first, base in seconds. This is just NTP->GPS base plus the week number multiplied by seconds in the week */
				/* FIXME: handle GPS week numbers which exist beyond 2037 */
				gps_s = (uint32_t) 315964819 + ((uint32_t) ((pkt[5] << 8)+pkt[6]) * (uint32_t) 604800);

				/* now apply the time of week to it */
				{
					double f; /* AVR-libc doubles are actually single precision */

					/* hack: byte copy into a float */
					/* AVR are LSB first, wire is MSB first */
					*((uint8_t *) &f) = pkt[4];
					*((uint8_t *) &f + 1) = pkt[3];
					*((uint8_t *) &f + 2) = pkt[2];
					*((uint8_t *) &f + 3) = pkt[1];

					gps_s += (uint32_t) trunc(f);

				//	serial0_tx_PGM(PSTR("gps f="));
				//	serial0_tx_dec((uint32_t)f);
				//	serial0_tx_cr();

					/* and now multiple out the 1000ths of a second to store too */
					//		 clock_ms_gps = ((uint32_t) (f * 1000.0)) % 1000;
					//rtc_ms_gps = 0; /* it will always be 0 */

#if 1
					char str[64] = "";

					snprintf(str,63,"gps: Week %d TOW %ld\r\n",((pkt[5] << 8)+pkt[6]),(uint32_t)trunc(f));
					serial0_tx(str,strlen(str));
#endif
				}
				/* and now apply the UTC leap seconds correction */
				{
					float f;

					/* hack: byte copy into a float */
					/* AVR are LSB first, wire is MSB first */
					*((uint8_t *) &f) = pkt[10];
					*((uint8_t *) &f + 1) = pkt[9];
					*((uint8_t *) &f + 2) = pkt[8];
					*((uint8_t *) &f + 3) = pkt[7];
#if 1
					serial0_tx_PGM(PSTR("gps: offset is "));
					serial0_tx_dec((uint32_t) f);
					serial0_tx_cr();
#endif

					/* UTC offset is always integer second even tho we get a float, go figure */
					gps_s -= (uint32_t) f;
				}

				/* FIXME: some sort of conversion needed here, looks awfully like TAI since it's 19s but makes no sense.. */
				gps_s -= 19;

				/* indicate we have the current GPS time */
				set_flag(flag_gps,FLAG_GPS_EVENT_TIME);

				global_counters.gps_frame_good++;

				break;
#endif
			case 0x45:
				if (n != 11) {
					serial0_tx_PGM(PSTR("gps: rx bad software version packet, disconnecting GPS\r\n"));
					clear_flag(flag_gps,FLAG_GPS_GOOD);
					global_counters.gps_frame_bad++;
					return;
				}
				if (!(flag_gps & FLAG_GPS_GOOD)) {
					serial0_tx_PGM(PSTR("gps: sending hw request\r\n"));
					/* this is a startup packet, request hw ID */
					serial1_tx_PGM(PSTR("\x10\x1c\x03\x10\x03"));
				}
				global_counters.gps_frame_good++;
				break;
			case 0x46:
				if (n != 3) {
					/* getting strange 0x46 packets that are short? */
					global_counters.gps_frame_bad++;
					break;
				}
				if (!(flag_gps & FLAG_GPS_GOOD)) {
					serial0_tx_PGM(PSTR("gps: ignoring receiver health status until config complete\r\n"));
					global_counters.gps_frame_ignored++;
					break;
				}
				/* receiver health status */
				/* undocumented: Copernicus 1 modules can do stationary, and report lock as bb */
				if ((pkt[1] == 0 || pkt[1] == 0xbb) && !(flag_gps & FLAG_GPS_LOCK)) {
					set_flag(flag_gps, FLAG_GPS_LOCK | FLAG_GPS_EVENT_LOCK);
					serial0_tx_PGM(PSTR("gps: locked\r\n"));
					//PORTB |= (1 << PORTB1);
				}
				if (pkt[1] != 0 && pkt[1] != 0xbb && (flag_gps & FLAG_GPS_LOCK)) {
					clear_flag(flag_gps, FLAG_GPS_LOCK | FLAG_GPS_EVENT_LOCK);
					serial0_tx_PGM(PSTR("gps: lost lock "));
					serial0_tx_hex(pkt[1]);
					serial0_tx_cout('\r');
					serial0_tx_cout('\n');
					//PORTB &= ~(1 << PORTB1);
				}
				global_counters.gps_frame_good++;
				break;
#ifdef DEBUG_GPS
			case 0x4a:
			{
				uint8_t x;
				char str[20] = "";
				float latitude, longitude, altitude;

				/* Single-precision LLA position */
				/* must be a better way to do this */
				*((uint8_t *) &latitude) = pkt[4];
				*((uint8_t *) &latitude + 1) = pkt[3];
				*((uint8_t *) &latitude + 2) = pkt[2];
				*((uint8_t *) &latitude + 3) = pkt[1];

				/* convert to degrees */
				latitude = latitude * (180.0 / 3.1415926535898);
				snprintf(str,19,"%.5f",(double)latitude);
				serial0_tx_PGM(PSTR("gps: lat "));
				serial0_tx(str,strlen(str));

				*((uint8_t *) &longitude) = pkt[8];
				*((uint8_t *) &longitude + 1) = pkt[7];
				*((uint8_t *) &longitude + 2) = pkt[6];
				*((uint8_t *) &longitude + 3) = pkt[5];

				/* convert to degrees */
				longitude = longitude * (180.0 / 3.1415926535898);
				snprintf(str,19,"%.5f",(double)longitude);
				serial0_tx_PGM(PSTR(" long "));
				serial0_tx(str,strlen(str));
				serial0_tx_cr();

				global_counters.gps_frame_good++;
				break;
			}
#endif
			case 0x55:
				/* IO Options Report */
				/* pg 136 */
				/* Strictly speaking, we should compare this to what we feed in the init phase */
				/* fixme: identify options applied, figure out why settings not being accepted at all */
				//serial0_tx_PGM(PSTR("gps: settings accepted\r\n"));
				//set_flag(flag_gps,FLAG_GPS_GOOD);
				break;
			case 0x58:
				/* Satellite parameters request response. This should only be seen in response to issuing a 0x38 request */
				if (!(flag_gps & FLAG_GPS_GOOD)) {
					/* ignore the frame since we're not certain of it's validity */
					global_counters.gps_frame_ignored++;
					break;
				}
				/* we are only interested in UTC reports */
				if (pkt[1] != 0x1 || pkt[2] != 0x5) { /* 0x1 == Request Data, 0x5 == UTC data type */
					global_counters.gps_frame_ignored++;
					break;
				}
				/* this is an interesting packet to us, store the expected leap second insertion date */
				/* packet offsets in payload (not including command)
				 * 29-30 delta_t_ls = current offset
				 * 35-36 wn t = current week number
				 * 37-38 wn_LSF = week number of offset to apply on
				 * 39-40 DN = day number of week that the offset kicks in
				 * 41-42 delta_t_lsf = future offset
				 */
				/* determine if there is a leap second in the next 24 hours */

				serial0_tx_PGM(PSTR("UTC "));
				serial0_tx_hex(pkt[30]);
				serial0_tx_hex(pkt[31]);
				serial0_tx_cout(' ');
				serial0_tx_hex(pkt[36]);
				serial0_tx_hex(pkt[37]);
				serial0_tx_cout(' ');
				serial0_tx_hex(pkt[38]);
				serial0_tx_hex(pkt[39]);
				serial0_tx_cout(' ');
				serial0_tx_hex(pkt[40]);
				serial0_tx_hex(pkt[41]);
				serial0_tx_cout(' ');
				serial0_tx_hex(pkt[42]);
				serial0_tx_hex(pkt[43]);
				serial0_tx_cr();
				break;
			case 0x6D:
				/* All-in-view Satellite Selection */
				/* Pg 144 */
				if (!(flag_gps & FLAG_GPS_GOOD)) {
#ifdef DEBUG_GPS
					serial0_tx_PGM(PSTR("gps: ignoring all-in-view report until config complete\r\n"));
#endif
					global_counters.gps_frame_ignored++;
					break;
				}

				/* update satellite count */
				gps_sat_count = (pkt[1] & 0x7);

				//		if (!(flags & FLAG_GPS_EVENT_LOCK)) {
				//		 break;
				//		}
				//		clear_flag(FLAG_GPS_EVENT_LOCK);

				//		serial0_tx_PGM(PSTR("gps: "));
				//		if (pkt[1] & 0x7) {
				//		 serial0_tx_dec((pkt[1] & 0x7) - 1);
				//		 serial0_tx_PGM(PSTR("D fix ("));
				//		 serial0_tx_dec(pkt[1] >> 4);
				//		 serial0_tx_PGM(PSTR(" satellites)\r\n"));
				//		} else {
				//		 serial0_tx_PGM(PSTR("no fix\r\n"));
				//		}
				global_counters.gps_frame_good++;
				break;
			case 0x8f:
				/* TSIP superpacket processing */
				/* Superpackets have an additional processing value. We only care about 0x20 ones */
				if (pkt[1] != 0x20) {
					break;
				}
				/* compact position fix is 0x20, contains current GPS time in integer formats */
				{
#ifdef DEBUG_GPS
					char str[64] = "";
#endif
					uint32_t tow;
					uint16_t week;
					uint8_t leap;

					/* convert fixed point numbers to our bases */
					/* time of week (milliseconds, uint32) */
					tow = (((uint32_t)pkt[9]<<24) | ((uint32_t)pkt[10]<<16) | ((uint32_t)pkt[11]<<8) | ((uint32_t)pkt[12])) / 1000UL;
					/* gps week number (uint16) */
					week = ((pkt[31] << 8) | pkt[32]);
					/* utc leap seconds (uint8) */
					leap = pkt[30];
#ifdef DEBUG_GPS
					snprintf(str,63,"gps: Week %u TOW %lu leap %u\r\n",week,tow,leap);
					serial0_tx(str,strlen(str));
#endif
					/* multiply out the current GPS time */
					gps_s = (week * 604800UL) + tow;
					/* store the leap seconds as well, for conversion to UTC */
					gps_leap = leap;
				}
//				/* UTC to unix time for NTP */
//				gps_s += 315964819UL;
//
//				/* FIXME: this adjustment is needed why? */
//				gps_s -= 20UL;

				/* indicate we have the current GPS time */
				set_flag(flag_gps,FLAG_GPS_EVENT_TIME);

				global_counters.gps_frame_good++;

				break;
			default:
				global_counters.gps_frame_ignored++;
		}
		/* reset state */
		state = GPS_STATE_START;
		n = 0;
	}

	return;

}

/* quickly just go into passthru mode and stay there until the magic received sequence +++ is sent */
void gps_passthru(void) {
	char s;
	uint8_t esc = 0;

	serial0_tx_PGM(PSTR("gps: passthru mode active, send +++ to exit\r\n"));

	/* disable echo */
	serial0_echo(0);

	while (1) {
		wdt_reset();
		if (flag_serial & FLAG_SERIAL_S0_RX) {
			s = serial0_rx();
			if (s == '+') {
				esc++;
				if (esc > 2) {
					break;
				}
			} else {
			 esc = 0;
			}
			serial1_tx_cout(s);
		}
		if (flag_serial & FLAG_SERIAL_S1_RX) {
			serial0_tx_cout(serial1_rx());
		}
	}

	serial0_echo(1);
	serial0_tx_PGM(PSTR("gps: exiting passthru \r\n"));
	gps_init();
	return;
}
